#include "Controller.h"
#include "Telescope.h"
#include "NexStar.h"

void nexstar_start(Telescope *telescope) {
	struct termios options;
	speed_t baudrate = B9600;
	cfsetospeed(&options, (speed_t) baudrate);
	cfsetispeed(&options, (speed_t) baudrate);
	options.c_cflag &= ~PARENB;
	options.c_cflag &= ~CSTOPB;
	options.c_cflag &= ~CSIZE;
	options.c_cflag |= CS8;
	options.c_cflag |= CLOCAL | CREAD;
	options.c_cflag &= ~CRTSCTS;
	options.c_iflag &= ~(IXON|IXOFF|IXANY);
	options.c_lflag = 0;
	options.c_oflag = 0;
	options.c_cc[VTIME] = 30;
	options.c_cc[VMIN] = 0;
//	options.c_cc[VTIME] = 1;
//	options.c_cc[VMIN] = 60;
	tcsetattr(telescope->sdd, TCSANOW, &options);
	printf("Nextstar telescope is up...\n");
}

void nexstar_execute_command(Telescope *telescope, Client *client, uint8_t command, char *params, char *callback) {
	char response[128];
	switch(command) {
		case GET_AZ_ALT:
			printf("Executing GET AZ/ALT...\n");
			nexstar_get_az_alt(telescope, callback, response);
			break;
		case GET_RA_DEC:
			printf("Executing GET RA/DEC...\n");
			nexstar_get_ra_dec(telescope, callback, response);
			break;
		case GOTO_AZ_ALT:
			printf("Executing GOTO AZ/ALT...\n");
			nexstar_goto_az_alt(telescope, params, callback, response);
			break;
		case GOTO_RA_DEC:
			printf("Executing GOTO RA/DEC...\n");
			nexstar_goto_ra_dec(telescope, params, callback, response);
			break;
		default:
			Throw 500;
			break;
	}
	network_send_response(client, response);
	close(telescope->sdd);
	nexstar_start(telescope);
}

void nexstar_get_az_alt(Telescope *telescope, char *callback, char *response) {
	char data[16] = "Z";
	int minutes, az[3], alt[3], value;
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, data);
	sscanf(data, "%2x%2x,%2x%2x#", az[0], az[1], alt[0], alt[1]);
	value = 256*az[0] + az[1];
	minutes = value/3/F;
	az[0] = (int) minutes/60;
	az[1] = (int) minutes%60;
	az[2] = 60*(minutes%60 - dec[1]);
	value = 256*alt[0] + alt[1];
	minutes = value/3/F;
	alt[0] = (int) minutes/60;
	alt[1] = (int) minutes%60;
	alt[2] = 60*(minutes%60 - alt[1]);
	sprintf(response, "%s({\"az\":\"%dd%dam%0.2fas\",\"alt\":\"%dd%dam%0.2fas\"})", callback, az[0], az[1], az[2], alt[0], alt[1], alt[2]);
}

void nexstar_get_ra_dec(Telescope *telescope, char *callback, char *response) {
	char data[16] = "E";
	int minutes, ra[3], dec[3], value;
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, data);
	sscanf(data, "%2x%2x,%2x%2x#", ra[0], ra[1], dec[0], dec[1]);
	value = 256*ra[0] + ra[1];
	minutes = value/3/15/F;
	ra[0] = (int) minutes/60;
	ra[1] = (int) minutes%60;
	ra[2] = 60*(minutes%60 - ra[1]);
	value = 256*dec[0] + dec[1];
	minutes = value/3/F;
	dec[0] = (int) minutes/60;
	dec[0] = (dec[0] > 90)? dec[0] - 360 : dec[0];
	dec[1] = (int) minutes%60;
	dec[2] = 60*(minutes%60 - dec[1]);
	sprintf(response, "%s({\"ra\":\"%dh%dm%0.2fs\",\"dec\":\"%dd%dam%0.2fas\"})", callback, ra[0], ra[1], ra[2], dec[0], dec[1], dec[2]);
}

void nexstar_goto_az_alt(Telescope *telescope, char *params, char *callback, char *response) {
	char data[16];
	float ra[3], alt[3];
	int high, low, minutes, value;
	sscanf(params, "%f+%f+%f+%f+%f+%f", &az[0], &az[1], &az[2], &alt[0], &alt[1], &alt[2]);
	minutes = (int) (60*az[0] + az[1] + az[2]/60);
	value = (int) 3*F*minutes;
	high = (int) value/256;
	low = value%256;
	sprintf(data, "R%x%x", high, low);
	minutes = (int) (60*alt[0] + alt[1] + alt[2]/60);
	value = (int) 3*f*minutes;
	high = (int) value/256;
	low = value%256;
	sprintf(data, "%s,%x%x", high, low);
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, data);
	if(data[0] == '#')
		sprintf(response, "%s({\"#\"})", callback);
	else Throw 500;
}

void nexstar_goto_ra_dec(Telescope *telescope, char *params, char *callback, char *response) {
	char data[16];
	float ra[3], dec[3];
	int high, low, minutes, value;
	sscanf(params, "%f+%f+%f+%f+%f+%f", &ra[0], &ra[1], &ra[2], &dec[0], &dec[1], &dec[2]);
	minutes = (int) 15*(60*ra[0] + ra[1] + ra[2]/60);
	value = (int) 3*F*minutes;
	high = (int) value/256;
	low = value%256;
	sprintf(data, "R%x%x", high, low);
	dec[0] = (dec[0] < 0)? 360 + dec[0]: dec[0];
	minutes = (int) (60*dec[0] + dec[1] + dec[2]/60);
	value = (int) 3*f*minutes;
	high = (int) value/256;
	low = value%256;
	sprintf(data, "%s,%x%x", high, low);
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, data);
	if(data[0] == '#')
		sprintf(response, "%s({\"#\"})", callback);
	else Throw 500;
}

/*
void nexstar_get_ra_dec(Telescope *telescope, char *callback, char *response) {
	char data[128] = "E", ra[32], dec[32];
	uint16_t rahex, dechex;
	telescope_write_data(telescope, data);
	telescope_read_data(telescope, data);
	sscanf(data, "%hx,%hx#", &rahex, &dechex);
	nexstar_convert_hex_to_hrs(rahex, ra);
	nexstar_convert_hex_to_dgs(dechex, dec);
	sprintf(response, "%s({\"ra\":\"%s\",\"dec\":\"%s\"})", callback, ra, dec);
}

void nexstar_goto_ra_dec(Telescope *telescope, char *params, char *callback, char *response) {
	char data[10], ra[5], dec[5];
	float hrs[3], dgs[3];
	sscanf(params, "%f+%f+%f+%f+%f+%f", &hrs[0], &hrs[1], &hrs[2], &dgs[0], &dgs[1], &dgs[2]);
	nexstar_convert_hrs_to_hex(hrs, ra);
	nexstar_convert_dgs_to_hex(dgs, dec);
	sprintf(data, "R%s,%s", ra, dec);

	printf("%s\n", data);

	telescope_write_data(telescope, data);
	bzero(data, sizeof(data));
	//telescope_read_data(telescope, data);
	sprintf(response, "%s({\"\"})", callback);
}

void nexstar_convert_hex_to_dgs(uint16_t hex, char *deg) {
	float degrees = 360.0*(hex/65536.0);
	short int di = floor(degrees);
	short int mi = floor(60*(degrees - di));
	float si = roundf(60*(60*(degrees - di) - mi) * 100) / 100;
	sprintf(deg, "%hdd%hdam%0.2fas", di, mi, si);
}

void nexstar_convert_hex_to_hrs(uint16_t hex, char *hrs) {
	float hours = (360.0*(hex/65536.0))/15;
	uint16_t hi = floor(hours);
	uint16_t mi = floor(60*(hours - hi));
	float si = roundf(60*(60*(hours - hi) - mi) * 100) / 100;	
	sprintf(hrs, "%hdh%hdm%0.2fs", hi, mi, si);
}

void nexstar_convert_hrs_to_hex(float *hrs, char *hex) {
	float f = 15*hrs[0] + hrs[1]/4 + hrs[2]/240;
	uint16_t i = (uint16_t) floor(65535.0*(f/360.0));
	sprintf(hex, "%hx", i);
	hex[5] = '\0';
}

void nexstar_convert_dgs_to_hex(float *dgs, char *hex) {
	float f = dgs[0] + (dgs[1] + (dgs[2])/60.0)/60.0;
	uint16_t i = (uint16_t) 65535.0*(f/360.0);
	sprintf(hex, "%hx", i);
	hex[5] = '\0';
}
*/
